// 这个文件包含了需要添加到 omni_pid_pursuit_controller.cpp 中的修改

// ====== 在 configure 函数中添加 ======
// 在现有的参数初始化后添加：

  // Initialize altitude control variables
  target_altitude_ = 1.0;  // Default target altitude
  current_altitude_ = 0.0;
  
  // Create altitude subscriber
  auto node = node_.lock();
  altitude_sub_ = node->create_subscription<std_msgs::msg::Float64>(
    "/target_altitude", 
    10,
    std::bind(&OmniPidPursuitController::altitudeCallback, this, std::placeholders::_1)
  );
  
  // Create current pose publisher
  current_pose_pub_ = node->create_lifecycle_publisher<geometry_msgs::msg::PoseStamped>(
    "/drone_pose", 
    1
  );

// ====== 在 activate 函数中添加 ======
// 在现有的 on_activate 调用后添加：

  current_pose_pub_->on_activate();

// ====== 在 deactivate 函数中添加 ======
// 在现有的 on_deactivate 调用后添加：

  current_pose_pub_->on_deactivate();

// ====== 在 cleanup 函数中添加 ======
// 在现有的 reset 调用后添加：

  altitude_sub_.reset();
  current_pose_pub_.reset();

// ====== 在 computeVelocityCommands 函数中修改 ======
// 替换原有的高度控制逻辑：

  // 原来的代码：
  // double z_dist = carrot_pose.pose.position.z;
  
  // 新的代码：
  // Update current altitude from robot pose
  current_altitude_ = pose.pose.position.z;
  
  // Publish current position for monitoring
  publishCurrentPosition(pose);
  
  // Calculate altitude error using external target instead of path z-coordinate
  double z_dist = target_altitude_ - current_altitude_;

// ====== 添加新的方法实现 ======

void OmniPidPursuitController::altitudeCallback(const std_msgs::msg::Float64::SharedPtr msg)
{
  std::lock_guard<std::mutex> lock(mutex_);
  target_altitude_ = msg->data;
  RCLCPP_DEBUG(logger_, "Target altitude updated to: %.2f m", target_altitude_);
}

void OmniPidPursuitController::publishCurrentPosition(const geometry_msgs::msg::PoseStamped & pose)
{
  auto current_pose_msg = std::make_unique<geometry_msgs::msg::PoseStamped>();
  current_pose_msg->header = pose.header;
  current_pose_msg->pose = pose.pose;
  current_pose_pub_->publish(std::move(current_pose_msg));
} 